import argparse
import time
from simulation import CarlaSimulation

def main():
    # 解析命令行参数
    parser = argparse.ArgumentParser(description='CARLA MPC轨迹跟踪控制器')
    parser.add_argument('--town', type=str, default='Town05', help='CARLA地图名称')
    parser.add_argument('--host', type=str, default='localhost', help='CARLA服务器地址')
    parser.add_argument('--port', type=int, default=2000, help='CARLA服务器端口')
    parser.add_argument('--duration', type=int, default=30, help='仿真持续时间(秒)')
    parser.add_argument('--record', action='store_true', default=True, help='是否记录仿真数据')
    args = parser.parse_args()

    # 创建仿真实例
    sim = CarlaSimulation(
        town_name=args.town,
        host=args.host,
        port=args.port
    )
    sim.record_data = args.record

    try:
        # 连接到CARLA服务器
        if not sim.connect():
            print("无法连接到CARLA服务器，退出程序")
            return

        # 设置车辆
        if not sim.setup_vehicle():
            print("车辆设置失败，退出程序")
            return

        # 初始化MPC控制器
        sim.initialize_mpc()

        # 生成参考轨迹
        sim.generate_reference_trajectory()

        # 等待车辆稳定
        print("等待车辆稳定...")
        time.sleep(2)

        # 运行仿真
        print(f"开始仿真，持续时间: {args.duration}秒")
        success = sim.run_simulation(duration=args.duration)

        if success:
            print("仿真运行成功")
        else:
            print("仿真运行失败")

    except KeyboardInterrupt:
        print("用户中断仿真")
    finally:
        # 清理仿真环境
        sim.cleanup()
        print("程序退出")

if __name__ == '__main__':
    main()